#pragma config(Sensor, in1,    TurnPot,        sensorPotentiometer)
#pragma config(Sensor, dgtl1,  ArmEncoderLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  ArmEncoderRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseEncoderLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  BaseEncoderRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  SwitchToLargeGoal, sensorTouch)
#pragma config(Sensor, dgtl10, SwitchToSmallGoal, sensorTouch)
#pragma config(Sensor, dgtl11, SwitchVacume,   sensorTouch)
#pragma config(Sensor, dgtl12, AltButton,      sensorTouch)
#pragma config(Motor,  port1,           LeftBase,      tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port2,           LeftArm,       tmotorVex393, openLoop)
#pragma config(Motor,  port5,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           LeftVacume,    tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port7,           RightVacume,   tmotorVex269, openLoop)
#pragma config(Motor,  port8,           LeftTurntable, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port9,           RightTurntable, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port10,          RightBase,     tmotorVex393HighSpeed, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

const int turnstraight = 1900;
const int maxturnforward = 783;
const int maxturnbackward = 3423;
const int scoremediumturn = 1780;
const int pickupball = 2580;
// Start Autonomous Voids
const int HalfAHair = 2;
const int SquareAndThreeQuarters = 38;
const int SquareAndAHalf = 55;
const int Square = 23.187; // 35?
const int ToMediumGoal = 110; // 2.5 squares = 57.5 in
const int BackToMediumGoal = -120;
const int MaxArms = 2250;
const int ToLargeGoal = 135;
const int BackToInteractonTile = -145;
const int BackToIsoTile = -117;
// new autonomous
const int ToSmallGoal = 90; //23;
const int BackToSmallGoal = -90; //-23;
const int ToDoubler = 400; //210;
const int BackToDoubler = -410; //-220;
const int tocorner = -310;
const int backtocorner = 310;
int motorpowerright = 0;
int motorpowerleft = 0;
bool VirtualButton = false;
int Turntablepos = 0;
int turntableturned = 0;

///////////////////////////////////
// This makes sure the arms don't /
// sink when not going up or down /
///////////////////////////////////
void armspower()
{
  motor(LeftArm) = 10;
  motor(RightArm) = 10;
}
/////////////////////////////
// Resets the Base encoders /
/////////////////////////////
void ResetEncoders()
{
  SensorValue(BaseEncoderLeft) = 0;
  SensorValue(BaseEncoderRight) = 0;
}
/////////////////////////////
//  Resets the Arm Encoders /
/////////////////////////////
void ResetArmEncoders()
{
  SensorValue(ArmEncoderLeft) = 0;
  SensorValue(ArmEncoderRight) = 0;
}
/////////////////////////////////////////////////
// Finds the distance based on encoder counts ///
/////////////////////////////////////////////////
int DistanceToCounts(int distance)//Distance in 1/10 inches!
{
  const int circumference = 12.6;//Large omni this variable can only go to tenths decimal
  int counts = 0;//1 count = 4 degrees, initially set to zero

  counts = (distance * 90) / (circumference);//Set counts for proper distance
  return counts;
}
////////////////////////////////
// Makes the base motors stop //
////////////////////////////////
void DriveMotorStop()
{
  motor[LeftBase] = 0;
  motor[RightBase] = 0;
}
////////////////////////////////////////////////
// Runs the vacume to pick up a ball or barrel /
////////////////////////////////////////////////
void VacumePickUp()
{
  motor[LeftVacume] = 127;
  motor[RightVacume] = 127;
  wait1Msec(600);
  motor[LeftVacume] = 0;
  motor[RightVacume] = 0;
}
//////////////////////////////////////////////
// Runs the vacume to score a ball or barrel /
//////////////////////////////////////////////
void VacumeSpitOut()
{
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    motor[LeftArm] = 10;
    motor[RightArm] = 10;
    wait1Msec(3500);
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
    motor[LeftArm] = 0;
    motor[RightArm] = 0;
}
void VacumeSpitOutIso()
{
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    motor[LeftArm] = 10;
    motor[RightArm] = 10;
    wait1Msec(1000);
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
    motor[LeftArm] = 0;
    motor[RightArm] = 0;
}
//////////////////////////////////////////////////
// Runs the arms until the arms reach the height /
// of the 30 inch goal                           /
//////////////////////////////////////////////////
void ArmsTo30()
{
  if(SensorValue(ArmEncoderLeft) < 1000)
  {
  bool ArmUp = true;
  while(ArmUp == true)
  {
    if(SensorValue(ArmEncoderLeft) <= 2100)
    {
      motor[LeftArm] = 127;
    }
    else
    {
      motor[LeftArm] = 10;
    }
    if(SensorValue(ArmEncoderRight) <= 2100)
    {
      motor[RightArm] = 127;
    }
    else
    {
      motor[RightArm] = 10;
    }
    if(SensorValue(ArmEncoderLeft) >= 2100 && SensorValue(ArmEncoderRight) >= 2100)
    {
      ArmUp = false;
    }
  }
}
  motor[RightArm] = 10;
  motor[LeftArm] = 10;
}

///////////////////////////////////////////////////////////////
// Runs the robot base straight until the distance is reached /
///////////////////////////////////////////////////////////////
void MoveStraight(int distance)
{
 bool stall = false;
 int counts;//This is a variable to hold distance in counts
 ResetEncoders();//Reset encoders with function, start at zero!
 counts = DistanceToCounts(distance);
  while (SensorValue(BaseEncoderRight)<counts && stall == false)
  {
    armspower();
    if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Left faster
    {
      motor[LeftBase]=117;
      motor[RightBase]=127;
    }
    if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Right faster
    {
      motor[LeftBase]=127;
      motor[RightBase]=117;
    }
    if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
    {
      motor[LeftBase]=127;
      motor[RightBase]=127;
    }

  }
  DriveMotorStop();
}
///////////////////////////////
// Same as above but backward /
///////////////////////////////
void MoveStraightBackward(int distancebackward)
{
   int counts;//This is a variable to hold distance in counts
   ResetEncoders();//Reset encoders with function, start at zero!
   counts = DistanceToCounts(distancebackward);
    {
    while (SensorValue(BaseEncoderRight)>counts)
    {
      armspower();
      if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Left faster
      {
        motor[LeftBase]=-93;
        motor[RightBase]=-103;
      }
      if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Right faster
      {
        motor[LeftBase]=-103;
        motor[RightBase]=-93;
      }
      if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
      {
        motor[LeftBase]=-103;
        motor[RightBase]=-103;
      }
    }
    DriveMotorStop();
  }
}

/////////////////////////////////////
// same as above but for the corner /
/////////////////////////////////////
void MoveStraightBackwardCorner(int distancebackward)
{
   int counts;//This is a variable to hold distance in counts
   ResetEncoders();//Reset encoders with function, start at zero!
   counts = DistanceToCounts(distancebackward);
    {
    while (SensorValue(BaseEncoderRight)>counts)
    {
      armspower();
      if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Left faster
      {
        motor[LeftBase]=-127;
        motor[RightBase]=-90;

      }
      if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Right faster
      {
        motor[LeftBase]=-90;
        motor[RightBase]=-127;
      }
      if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
      {
        motor[LeftBase]=-127;
        motor[RightBase]=-127;
      }
    }
    DriveMotorStop();
  }
}
////////////////////////////////////
// Moves the arms down to the base /
////////////////////////////////////
void MoveLiftDown()
{
  VirtualButton = true;
  while(VirtualButton == true)
  {
    if(SensorValue[ArmEncoderLeft] > 100)
    {
      motor[LeftArm] = -127;
    }
    if(SensorValue[ArmEncoderRight] > 100)
    {
      motor[RightArm] = -127;
    }
    if(SensorValue[ArmEncoderLeft] <= 100)
    {
      motor[LeftArm] = 0;
    }
    if(SensorValue[ArmEncoderRight] <= 100)
    {
      motor[RightArm] = 0;
    }
    if(SensorValue[ArmEncoderLeft] <= 100 && SensorValue[ArmEncoderRight] <= 100)
    {
      VirtualButton = false;
    }
  }
  ResetArmEncoders();
}
////////////////////////////////////////////////
// Turns the robot a certain number of degrees /
////////////////////////////////////////////////
void TurnRobot (int turnangle)
{
  int wheelseperation = 12;   //Width of robot base
  int rotationdistance;       //Distance wheel needs to turn in 1/10 inches
  int wheelcounts;            //Counts wheel needs to travel
  int absturnangle;           //Absalute value of turnangle
  bool negative = false;      //If a number is negative, this variable gets notated "true."

  absturnangle = 0;

  if(turnangle<0)//If turnangle variable is negative...
  {
    negative = true;//...Then notate it throught variables.
  }

  absturnangle = abs(turnangle);//Compute absolute value (-90 becomes 90 if negative or 90 stays 90)

  rotationdistance = (wheelseperation * absturnangle * 3.14159265) / 36;  //Compute distance in 1/10"; PI = 3.14159265 here
  wheelcounts = DistanceToCounts(rotationdistance);  //Turn distance into counts
  bMotorFlippedMode[port3] = true;  //No reflection

  ResetEncoders();  //Reset encoders for use.

  if(negative == false)
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)  //Runs when needed.
    {
      motor[LeftBase] = -63;
      motor[RightBase] = 63;
    }
  }
  else
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)
    {
      motor[LeftBase]= 63;
      motor[RightBase]= -63;
    }
  }
  DriveMotorStop();
}

//**Port Mapping**//
//All port 2's are left side
//All port 3's are right side

// End Autonomous Voids

// Start UserControl Voids
//////////////////////////////////////////
// Runs our turntable if certain buttons /
// are pressed                           /
//////////////////////////////////////////
void MainTurntable()
{
  if(vexRT[Btn6UXmtr2] == 1 && SensorValue(TurnPot) > maxturnforward + 300)
  {
    motor[LeftTurntable] = 127;
    motor[RightTurntable] = 127;
  }
  if(vexRT[Btn6UXmtr2] == 1 && SensorValue(TurnPot) < maxturnforward + 301 && SensorValue(TurnPot) > maxturnforward)
  {
    motor[LeftTurntable] = 20;
    motor[RightTurntable] = 20;
  }
  if(vexRT[Btn6UXmtr2] == 1 && SensorValue(TurnPot) < maxturnforward)
  {
    motor[LeftTurntable] = 10;
    motor[RightTurntable] = 10;
  }
  if(vexRT[Btn6UXmtr2] == 0 && vexRT[Btn6DXmtr2] == 0)
  {
    motor[LeftTurntable] = 10;
    motor[RightTurntable] = 10;
  }
  if(vexRT[Btn6DXmtr2] == 1 && SensorValue(TurnPot) < maxturnbackward - 300)
  {
    motor[LeftTurntable] = -127;
    motor[RightTurntable] = -127;
  }
  if(vexRT[Btn6DXmtr2] == 1 && SensorValue(TurnPot) > maxturnbackward - 301 && SensorValue(TurnPot) < maxturnbackward)
  {
    motor[LeftTurntable] = -20;
    motor[RightTurntable] = -20;
  }
  if(vexRT[Btn6DXmtr2] == 1 && SensorValue(TurnPot) > maxturnbackward)
  {
    motor[LeftTurntable] = 10;
    motor[RightTurntable] = 10;
  }
}
///////////////////////////////////////////////////
// Runs the turntable until in a scoring position /
///////////////////////////////////////////////////
void turntablebegining()
{
  motor[LeftTurntable] = 127;
  motor[RightTurntable] = 127;
  wait1Msec(600);
  motor[LeftTurntable] = 10;
  motor[RightTurntable] = 10;
}
int positiveright = 1;
int positiveleft = 1;

//////////////////////////////////////////////
// Controls the base with a ramping function /
//////////////////////////////////////////////
void MainBase()
{
  if(vexRT(Ch2) > 10)
  {
    positiveright = 1;
  }
  if(vexRT(Ch2) < -10)
  {
    positiveright = -1;
  }
  if(vexRT(Ch2) > 0 && vexRT(Ch2) < 10 || vexRT(Ch2) > -10 && vexRT(Ch2) <= 0)
  {
    positiveright = 0;
  }

  if(vexRT(Ch3) > 10)
  {
    positiveleft = 1;
  }
  if(vexRT(Ch3) < -10)
  {
    positiveleft = -1;
  }
  if(vexRT(Ch3) > 0 && vexRT(Ch3) < 10 || vexRT(Ch3) > -10 && vexRT(Ch3) <= 0)
  {
    positiveleft = 0;
  }


if(positiveright == 1 && motorpowerright <= 100)
{
  motorpowerright = motorpowerright + 5;
  wait1Msec(10);
}

if(positiveleft == 1 && motorpowerleft <= 100)
{
  motorpowerleft = motorpowerleft + 5;
  wait1Msec(10);
}
if(positiveright == -1 && motorpowerright >= -100)
{
  motorpowerright = motorpowerright - 5;
  wait1Msec(10);
}
if(positiveleft == -1 && motorpowerleft >= -100)
{
  motorpowerleft = motorpowerleft - 5;
  wait1Msec(10);
}
if(positiveright == 0)
{
  motor[RightBase] = 0;
  motorpowerright = 0;
}
if(positiveleft == 0)
{
  motor[LeftBase] = 0;
  motorpowerleft = 0;
}

  motor(LeftBase) = motorpowerleft;
  motor(RightBase) = motorpowerright;
}
////////////////////////////////
// Run the arms if they are OK /
////////////////////////////////
void ArmsOK()
{
  motor(LeftArm) = 127;
  motor(RightArm) = 127;
}
//////////////////////////////////////
// Stops the arms if they are not OK /
//////////////////////////////////////
void ArmsNotOK()
{
  motor(LeftArm) = 10;
  motor(RightArm) = 10;
}
//////////////////////////////////////
// Runs the arms down if they are OK /
//////////////////////////////////////
void ArmsOKDown()
{
  motor(LeftArm) = -127;
  motor(RightArm) = -127;
}
///////////////////////
// Same as armsto30() /
///////////////////////
void ArmsToTop()
{
  if(SensorValue(ArmEncoderLeft) <= 2100)  // makes the arms go down if Btn 7 Down is pressed
  {
    motor[LeftArm] = 127;
  }
  if(SensorValue(ArmEncoderLeft) >= 2100)
  {
    motor[LeftArm] = 10;
  }
  if(SensorValue(ArmEncoderRight) <= 2100)
  {
    motor[RightArm] = 127;
  }
  if(SensorValue(ArmEncoderRight) >= 2100)
  {
    motor[RightArm] = 10;
  }
}
/////////////////////////////////
// Runs the arms if they are OK /
/////////////////////////////////
void MainArms()
{
  if(vexRT(Btn7UXmtr2) == 1 && vexRT(Btn7DXmtr2) == 0)
  {
    if(SensorValue(ArmEncoderLeft) < 1900 && SensorValue(ArmEncoderRight) < 1900)
    {
      ArmsOK();
    }
    if(SensorValue(ArmEncoderLeft) > 1900 && SensorValue(ArmEncoderRight) > 1900 && SensorValue(ArmEncoderLeft) > 2100
      && SensorValue(ArmEncoderRight) > 2100)
    {
      ArmsNotOK();
    }
    if(SensorValue(ArmEncoderLeft) > 1900 && SensorValue(ArmEncoderRight) > 1900 && SensorValue(ArmEncoderLeft) < 2100
      && SensorValue(ArmEncoderRight) < 2100)
    {
      ArmsToTop();
    }
  }
  if(vexRT(Btn7UXmtr2) == 1 && vexRT(Btn7DXmtr2) == 1)
  {
    ArmsNotOK();
  }
  if(vexRT(Btn7DXmtr2) == 1 && vexRT(Btn7UXmtr2) == 0)
  {
    ArmsOKDown();
  }
  if(vexRT(Btn7DXmtr2) == 0 && vexRT(Btn7UXmtr2) == 0)
  {
    ArmsNotOK();
  }
}
///////////////////////////////////////////////////
// Runs the vacume if certain buttons are pressed /
///////////////////////////////////////////////////
void MainVacume()
{
  if(vexRT[Btn8UXmtr2] == 1) // makes the vacume go up if Btn 8 up is pressed
  {
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
  }
  if(vexRT[Btn8UXmtr2] == 0 && vexRT[Btn8DXmtr2] == 0)
  {
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
  }
  if(vexRT[Btn8UXmtr2] == 1 && vexRT[Btn8DXmtr2] == 1)
  {
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
  }
  if(vexRT[Btn8DXmtr2] == 1)
  {
    motor[LeftVacume] = -127;
    motor[RightVacume] = -127;
  }
}
/////////////////////////////////////////
// Runs the vacume while going straight /
/////////////////////////////////////////
void vacumedoubler(int distance)
{
 int counts;//This is a variable to hold distance in counts
 ResetEncoders();//Reset encoders with function, start at zero!
 counts = DistanceToCounts(distance);
  while (SensorValue(BaseEncoderRight)<counts)
  {
    armspower();
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Left faster
    {
      motor[LeftBase]=93;
      motor[RightBase]=103;
    }
    if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Right faster
    {
      motor[LeftBase]=103;
      motor[RightBase]=93;
    }
    if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
    {
      motor[LeftBase]=103;
      motor[RightBase]=103;
    }
  }
  DriveMotorStop();
}
////////////////////////////////////////////
// Turns the turntable to scoring posision /
////////////////////////////////////////////
void turntablescore()
{
  while(SensorValue(TurnPot) > scoremediumturn)
  {
    motor(LeftTurntable) = 127;
    motor(RightTurntable) = 127;
  }
    motor(LeftTurntable) = 10;
    motor(RightTurntable) = 10;
}
///////////////////////////////////////
// Scores in the large goal if called /
///////////////////////////////////////
void GotoLargeGoal()
{
  bool alreadytogoal = false;
  if(Turntablepos == 0 && alreadytogoal == false)
  {
    MoveStraight(ToLargeGoal);
    ArmsTo30();
    turntablescore();
    Turntablepos = 1;
    VacumeSpitOut();
    MoveStraightBackward(BackToInteractonTile);
    alreadytogoal = true;
  }
  if(Turntablepos == 1 && alreadytogoal == false)
  {
    MoveStraight(ToLargeGoal);
    ArmsTo30();
    VacumeSpitOut();
    MoveStraightBackward(BackToInteractonTile);
    alreadytogoal = true;
  }
}
///////////////////////////////////////////////
// turns the turntable to score in small goal /
///////////////////////////////////////////////
void vacumescoresmall()
{
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    motor[LeftArm] = 10;
    motor[RightArm] = 10;
    wait1Msec(2000);
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
    motor[LeftArm] = 0;
    motor[RightArm] = 0;
}
///////////////////////////////////////
// Scores in the small goal if called /
///////////////////////////////////////
void GotoSmallGoal()
{
  bool alreadytogoal = false;
  if(Turntablepos == 1 && alreadytogoal == false)
  {
    MoveLiftDown();
    MoveStraight(ToSmallGoal);
    vacumescoresmall();
    MoveStraightBackward(BackToSmallGoal);
  }
  if(Turntablepos == 0 && alreadytogoal == false)
  {
    turntablebegining();
    MoveLiftDown();
    MoveStraight(ToSmallGoal);
    VacumeSpitOut();
    MoveStraightBackward(BackToSmallGoal);
  }

}
//////////////////////////////////////////
// Turns the turntable to pick up a ball /
//////////////////////////////////////////
void turntablepickupball()
{
  while(SensorValue(TurnPot) < pickupball)
  {
    motor(LeftTurntable) = -50;
    motor(RightTurntable) = -50;
  }
  motor(LeftTurntable) = 10;
  motor(RightTurntable) = 10;
}
void MoveToMedium()
{
  if(SensorValue(ArmEncoderLeft) < 1000)
  {
  bool ArmUp = true;
  while(ArmUp == true)
  {
    if(SensorValue(ArmEncoderLeft) <= 500)
    {
      motor[LeftArm] = 127;
    }
    else
    {
      motor[LeftArm] = 10;
    }
    if(SensorValue(ArmEncoderRight) <= 500)
    {
      motor[RightArm] = 127;
    }
    else
    {
      motor[RightArm] = 10;
    }
    if(SensorValue(ArmEncoderLeft) >= 500 && SensorValue(ArmEncoderRight) >= 500)
    {
      ArmUp = false;
    }
  }
}
  motor[RightArm] = 10;
  motor[LeftArm] = 10;
}
////////////////////////////////////////
// Scores in the medium goal if called /
////////////////////////////////////////
void MediumGoal()
{
  bool alreadytogoal = false;
  if(Turntablepos == 0 && alreadytogoal == false)
  {
    turntablescore();
    Turntablepos = 1;
    MoveStraight(ToMediumGoal);
    MoveToMedium();
    VacumeSpitOutIso();
    MoveStraightBackward(-HalfAHair);
    MoveLiftDown();
    turntablepickupball();
    VacumePickUp();
    //turntablescore();
    MoveStraightBackward(BackToMediumGoal);
    alreadytogoal = true;
  }
  if(Turntablepos == 1 && alreadytogoal == false)
  {
    MoveStraight(ToMediumGoal);
    MoveToMedium();
    MoveLiftDown();
    VacumeSpitOut();
    MoveStraightBackward(BackToMediumGoal);
    alreadytogoal = true;
  }
}

void PickUpIso()
{
  vacumedoubler(SquareAndAHalf);
  MoveStraightBackward(-SquareAndAHalf);
}

///////////////////////////////////
// Picks up the doubler if called /
///////////////////////////////////
void GotoDoubler()
{
  bool alreadytogoal = false;
  Turntablepos = 1;

  if(Turntablepos == 1 && alreadytogoal == false)
  {
    turntablepickupball();
    //Turntablepos = 0;
    turntablepickupball();
    vacumedoubler(ToDoubler);
    MoveStraight(ToDoubler);
    // we only want to move back 1 tile and rotate 90 degrees
    //MoveStraightBackward(100);
    //turnrobot90();
    //MoveStraightBackward(BackToDoubler);
    alreadytogoal = true;
  }
  if(Turntablepos == 0 && alreadytogoal == false)
  {
    //vacumedoubler(ToDoubler);
    //VacumePickUp();
    //MoveStraightBackward(BackToDoubler);
    //alreadytogoal = true;
  }
}
//////////////////////////////////
// Turns the vacume on if called /
//////////////////////////////////
void VacumeOn()
{
  motor(LeftVacume) = 127;
  motor(RightVacume) = 127;
}
///////////////////////////////////
// Turns the vacume off if called /
///////////////////////////////////
void VacumeOff()
{
  motor(LeftVacume) = 0;
  motor(RightVacume) = 0;
}
//////////////////////////////////
// Stops the motors if stalling //
//////////////////////////////////
task antistall()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(BaseEncoderLeft) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[LeftBase] > 20 || motor[LeftBase] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[LeftBase] > 20 || motor[LeftBase] < -20)
        {
        quadPrev = SensorValue(BaseEncoderLeft); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(BaseEncoderLeft)) < (abs(motor[LeftBase]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[LeftBase] = 0; // stops motor
          ClearTimer(T1);
          //hogCPU(); //stops task main from executing until the release button is pressed
          while(1)
          { //this button releases the motor
            motor[LeftBase] = 0; //stops the motor which is not running correctly
            motor[RightBase] = 0;
          }
          //releaseCPU(); //starts running task main again
        }
      }
    }
  }
}
//////////////////
// Same as above /
//////////////////
task antistallleft()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(BaseEncoderLeft) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[LeftBase] > 20 || motor[LeftBase] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[LeftBase] > 20 || motor[LeftBase] < -20)
        {
        quadPrev = SensorValue(BaseEncoderLeft); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(BaseEncoderLeft)) < (abs(motor[LeftBase]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[LeftBase] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
            motor[LeftBase] = 0; //stops the motor which is not running correctly
            motor[RightBase] = 0;
            MainArms(); // second remote
            MainVacume(); // second remote
            MainTurntable();
            //insert all user controls except for the motor that is being stopped
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}
//////////////////
// Same as above /
//////////////////
task antistallright()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(BaseEncoderRight) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[RightBase] > 20 || motor[RightBase] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[RightBase] > 20 || motor[RightBase] < -20)
        {
        quadPrev = SensorValue(BaseEncoderRight); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(BaseEncoderRight)) < (abs(motor[RightBase]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[RightBase] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
                motor[RightBase] = 0; //stops the motor which is not running correctly
                motor[LeftBase] = 0;
                MainArms(); // second remote
                MainVacume(); // second remote
                MainTurntable();
            //insert all user controls except for the motor that is being stopped
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}

// end UserControl Voids


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

//////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                           /
//                                 Autonomous Task                                           /
//                                                                                           /
// This task is used to control your robot during the autonomous phase of a VEX Competition. /
// You must modify the code to add your own robot specific commands here.                    /
//                                                                                           /
//////////////////////////////////////////////////////////////////////////////////////////////


task autonomous()
{
  // if the arms raising get out of sync by more than about 20 clicks - should
  // we just stop - because it is raising one side at fast rate and not the other
  StartTask(antistall);
  ResetArmEncoders();
 // GotoLargeGoal();
  while(1)
  {
    armspower();
	  if(SensorValue(SwitchToLargeGoal) == 1 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    GotoLargeGoal();
	  }
	  if(SensorValue(SwitchToSmallGoal) == 1 && SensorValue(AltButton) == 1)
	  {
	    turntablebegining();
      ArmsTo30();
      VacumeSpitOut();
	  }
	  if(SensorValue(SwitchToSmallGoal) == 1 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    GotoSmallGoal();
	  }
	  if(SensorValue(SwitchVacume) == 1 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    VacumeOn();
	  }
	  if(SensorValue(SwitchVacume) == 0 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    VacumeOff();
	  }
	  if(SensorValue(SwitchToLargeGoal) == 1 && SensorValue(AltButton) == 1) // alt pressed
	  {
	    PickUpIso();
	  }
	  if(SensorValue(SwitchVacume) == 1 && SensorValue(AltButton) == 1) // alt pressed
	  {
	    MediumGoal();
	  }
	 // if(SensorValue(SwitchToSmallGoal) == 1 && SensorValue(AltButton) == 1) // alt pressed
	  //{
	   // VirtualButton = false;
	    //while(Turntablepos == 1 && VirtualButton == false)
	    //{
	     // TurntableScoreSmall();
	     // Turntablepos = 0;
	     // VirtualButton = true;
	   // }
	   // while(Turntablepos == 0 && VirtualButton == false)
	   // {
	    //  TurntablePickupSmall();
	     // Turntablepos = 1;
	     // VirtualButton = true;
	   // }
	    //wait1Msec(1);
	  //}
	}

  while(1)
  {
    allMotorsOff();
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                             /
//                                 User Control Task                                           /
//                                                                                             /
// This task is used to control your robot during the user control phase of a VEX Competition. /
// You must modify the code to add your own robot specific commands here.                      /
//                                                                                             /
////////////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
  StopTask(antistall);
  //StartTask(antistallleft); stalls to easily durring competition
  //StartTask(antistallright);
  while(1) // the code goes forever and ever and ever... and EVER[][][][][][][][][][][][][] I like semi-colens; they are awesome;;;;;;;;;;;;;;;;;;;;;;;;
  { // This symbol is cool too! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    MainTurntable();
    MainBase();
    MainArms(); // second remote
    MainVacume(); // second remote
    wait1Msec(1);//needed for multitasking to work correctly
  }
}
